import numpy as np
import cv2
import matplotlib.pyplot as plt
import tqdm
from PIL import Image
from tools.visualizer.zwh_viewer import draw_boxes2img
import argparse
import copy
import os
import warnings
import cv2
import mmcv
import numpy as np
import torch
from mmcv import Config, DictAction
from mmcv.cnn import fuse_conv_bn
from mmcv.parallel import MMDataParallel, MMDistributedDataParallel
from mmcv.runner import (get_dist_info, init_dist, load_checkpoint,
                         wrap_fp16_model)

import mmdet
from mmdet3d.apis import single_gpu_test
from mmdet3d.datasets import build_dataloader, build_dataset
from mmdet3d.models import build_model
from mmdet.apis import multi_gpu_test, set_random_seed
from mmdet.datasets import replace_ImageToTensor
from tools.visualizer.zwh_viewer import color_mapping
# from mmdet3d_plugin.dev.models.transformer.transformer_lib import *
# from mmdet3d_plugin.dev.models.transformer.transformer_clean import *
from tools.visualizer.zwh_viewer import view_6imgs

if mmdet.__version__ > '2.23.0':
    # If mmdet version > 2.23.0, setup_multi_processes would be imported and
    # used from mmdet instead of mmdet3d.
    from mmdet.utils import setup_multi_processes
else:
    from mmdet3d.utils import setup_multi_processes

try:
    # If mmdet version > 2.23.0, compat_cfg would be imported and
    # used from mmdet instead of mmdet3d.
    from mmdet.utils import compat_cfg
except ImportError:
    from mmdet3d.utils import compat_cfg

def rotation_matrix_z(theta):
    """
    根据绕Z轴的角度theta（以弧度为单位）计算旋转矩阵。
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0],
                     [s, c, 0],
                     [0, 0, 1]])

def draw_boxes_on_bev(bev_img, boxes, color=(0, 255, 0), thickness=1, attrs=None):
    """
    在BEV图像上绘制3D bounding boxes。

    参数:
    bev_img (numpy.ndarray): 形状为(256, 256, 3)的BEV图像。
    boxes (list of list): 每个元素是一个[x, y, w, h, yaw]列表，表示一个3D bounding box。
    color (tuple): 绘制boxes时使用的颜色，格式为(B, G, R)。
    thickness (int): 绘制boxes时的线条粗细。
    """
    for idx, box in enumerate(boxes):
        color = tuple((color_mapping[attrs[idx] % len(color_mapping)]).tolist()) if attrs!=None else (0, 255, 0)

        x, y, w, h, yaw = box
        half_w, half_h = w / 2, h / 2

        # 计算四个角点（以box中心为原点，未旋转）
        corners = np.array([
            [-half_w, -half_h],
            [half_w, -half_h],
            [half_w, half_h],
            [-half_w, half_h]
        ])

        # 旋转矩阵（绕Z轴旋转）
        R = np.array([
            [np.cos(yaw), -np.sin(yaw)],
            [np.sin(yaw), np.cos(yaw)]
        ])

        # 旋转角点到正确的方向
        corners_rotated = (R @ corners.T).T

        # 平移到实际位置
        corners_translated = corners_rotated + [x, y]
        # corners_translated = corners_rotated + [y, x]

        # 转换为整数坐标（注意：这里可能需要处理超出图像边界的情况）
        corners_int = np.round(corners_translated).astype(int)

        # 绘制四边形（注意：这里假设四边形是直的，即不考虑透视效果）
        for i in range(4):
            j = (i + 1) % 4
            cv2.line(bev_img, tuple(corners_int[i]), tuple(corners_int[j]), color, thickness)


def parse_args():
    parser = argparse.ArgumentParser(
        description='MMDet test (and eval) a model')
    parser.add_argument('config', help='test config file path')
    parser.add_argument('checkpoint', help='checkpoint file')
    parser.add_argument('--out', help='output result file in pickle format')
    parser.add_argument(
        '--fuse-conv-bn',
        action='store_true',
        help='Whether to fuse conv and bn, this will slightly increase'
             'the inference speed')
    parser.add_argument(
        '--gpu-ids',
        type=int,
        nargs='+',
        help='(Deprecated, please use --gpu-id) ids of gpus to use '
             '(only applicable to non-distributed training)')
    parser.add_argument(
        '--gpu-id',
        type=int,
        default=0,
        help='id of gpu to use '
             '(only applicable to non-distributed testing)')
    parser.add_argument(
        '--format-only',
        action='store_true',
        help='Format the output results without perform evaluation. It is'
             'useful when you want to format the result to a specific format and '
             'submit it to the test server')
    parser.add_argument(
        '--data_type',
        type=str,
        default='cyw',
        choices=['cyw', 'dr', 'nus'],
        help='input data type')
    parser.add_argument(
        '--eval',
        type=str,
        nargs='+',
        help='evaluation metrics, which depends on the dataset, e.g., "bbox",'
             ' "segm", "proposal" for COCO, and "mAP", "recall" for PASCAL VOC')
    parser.add_argument('--show', action='store_true', help='show results')
    parser.add_argument(
        '--show-dir', default='work_dirs/test_view', help='directory where results will be saved')
    parser.add_argument(
        '--gpu-collect',
        action='store_true',
        help='whether to use gpu to collect results.')
    parser.add_argument(
        '--tmpdir',
        help='tmp directory used for collecting results from multiple '
             'workers, available when gpu-collect is not specified')
    parser.add_argument('--seed', type=int, default=0, help='random seed')
    parser.add_argument(
        '--deterministic',
        action='store_true',
        help='whether to set deterministic options for CUDNN backend.')
    parser.add_argument(
        '--cfg-options',
        nargs='+',
        action=DictAction,
        help='override some settings in the used config, the key-value pair '
             'in xxx=yyy format will be merged into config file. If the value to '
             'be overwritten is a list, it should be like key="[a,b]" or key=a,b '
             'It also allows nested list/tuple values, e.g. key="[(a,b),(c,d)]" '
             'Note that the quotation marks are necessary and that no white space '
             'is allowed.')
    parser.add_argument(
        '--options',
        nargs='+',
        action=DictAction,
        help='custom options for evaluation, the key-value pair in xxx=yyy '
             'format will be kwargs for dataset.evaluate() function (deprecate), '
             'change to --eval-options instead.')
    parser.add_argument(
        '--eval-options',
        nargs='+',
        action=DictAction,
        help='custom options for evaluation, the key-value pair in xxx=yyy '
             'format will be kwargs for dataset.evaluate() function')
    parser.add_argument(
        '--launcher',
        choices=['none', 'pytorch', 'slurm', 'mpi'],
        default='none',
        help='job launcher')
    parser.add_argument('--local_rank', type=int, default=0)

    parser.add_argument('--nms_pre', type=int, default=1000)
    parser.add_argument('--nms_thr', type=float, default=0.2)
    parser.add_argument('--score_thr', type=float, default=0.1)

    args = parser.parse_args()
    if 'LOCAL_RANK' not in os.environ:
        os.environ['LOCAL_RANK'] = str(args.local_rank)

    if args.options and args.eval_options:
        raise ValueError(
            '--options and --eval-options cannot be both specified, '
            '--options is deprecated in favor of --eval-options')
    if args.options:
        warnings.warn('--options is deprecated in favor of --eval-options')
        args.eval_options = args.options
    return args


def featmap_view(model,
                 data_loader,
                 show=False,
                 out_dir=None,
                 show_score_thr=0.3,
                 pth_dir=None,
                 data_type='cyw',
                 rotate2base=0.0,
                 offset_z=None):
    if data_type == 'cyw' or data_type == 'dr':
        cams = [None,'camera75',None,'camera81','camera77','camera80']
        piece_height = 720
        piece_width = 1280
    else:
        cams = ['CAM_FRONT_LEFT', 'CAM_FRONT', 'CAM_FRONT_RIGHT',
                'CAM_BACK_LEFT', 'CAM_BACK', 'CAM_BACK_RIGHT']
        piece_height = 900
        piece_width = 1600
    model.eval()
    results = []
    dataset = data_loader.dataset
    prog_bar = mmcv.ProgressBar(len(dataset))

    # 图片的布局
    big_map_height = piece_height * 2
    big_map_width = piece_width * 4

    frame_rate = 5  # 每秒帧数
    video_h = int(piece_height*0.5)
    video_w = piece_width
    if not os.path.exists(out_dir):
        os.mkdir(out_dir)
    output_file = os.path.join(out_dir, 'output.mp4')  # 输出文件名
    fourcc = cv2.VideoWriter_fourcc(*'mp4v')  # 设置视频编码格式（这里使用 MP4V 编码）
    out = cv2.VideoWriter(output_file, fourcc, frame_rate, (video_w, video_h))

    for i, data in enumerate(tqdm.tqdm(data_loader)):
        # if i<300:
        #     continue
        if i>300:
            break
        # if i < 10 or i%20!=0:
        #     continue
        with torch.no_grad():
            points = []
            points.append(data['points'][0].data[0][0].to('cuda:0'))
            radar = []
            if 'pts_radar' in data.keys():
                radar.append(data['pts_radar'][0].data[0][0].to('cuda:0'))


            result, bev = model(return_loss=False, forward_video=True, **data)


        labels = result[0]['pts_bbox']['labels_3d']
        mask = torch.logical_and((labels>-1) , result[0]['pts_bbox']['scores_3d']> show_score_thr) # view all class
        # mask = (labels <= 4) | (labels == 8)
        result[0]['pts_bbox']['labels_3d'] = result[0]['pts_bbox']['labels_3d'][mask]
        result[0]['pts_bbox']['scores_3d'] = result[0]['pts_bbox']['scores_3d'][mask]
        result[0]['pts_bbox']['boxes_3d'] = result[0]['pts_bbox']['boxes_3d'][mask]
        result[0]['pts_bbox']['attrs_3d'] = result[0]['pts_bbox']['attrs_3d'][mask]

        attrs_3d = result[0]['pts_bbox']['attrs_3d']
        boxes = result[0]['pts_bbox']['boxes_3d'].tensor.numpy()[:,:7]

        boxes[:, 2] = boxes[:, 2] + 0.5 * boxes[:, 5]
        if offset_z is not None:
            boxes[:,2]=-offset_z[data_type]

        box2base = np.eye(4)
        box2base[:3,:3] = rotation_matrix_z(rotate2base)

        # prepare img + bev
        top_lefts = [[0,0],[0,1],[0,2],
                     [1,0],[1,1],[1,2]]
        big_map = np.zeros((big_map_height, big_map_width,3), dtype=np.uint8)
        # valid_cams = data['img_metas'][0].data[0][0]['img_info'].keys()
        for idx in range(len(cams)):
            # if cams[idx] not in valid_cams:
            #     cams[idx] = None
            #     continue
            if cams[idx] is None:
                continue

            file_id = -1
            for cid, cam_file in enumerate(data['img_metas'][0].data[0][0]['filename']):
                if cams[idx] in cam_file:
                    file_id = cid
                    break

            filename = data['img_metas'][0].data[0][0]['filename'][file_id]
            if not os.path.exists(filename):
                filename = filename.replace('_normal', '')
                assert os.path.exists(filename), '{} not exist'.format(filename)

            img = cv2.imread(filename)
            lidar2img = data['img_metas'][0].data[0][0]['lidar2img_ori'][file_id]

            # img = cv2.resize(img, (piece_width, piece_height), interpolation=cv2.INTER_AREA)

            img = draw_boxes2img(boxes=boxes,image=img,tfs=[box2base, lidar2img],attrs=attrs_3d)

            # cv2.imshow('ss',img)
            # cv2.waitKey()

            top_left = top_lefts[idx]
            big_map[top_left[0]*piece_height:top_left[0]*piece_height+piece_height,top_left[1]*piece_width:top_left[1]*piece_width+piece_width] = img

        bev_show = cv2.resize(bev, (big_map_height, big_map_height), interpolation=cv2.INTER_AREA)
        bev_box = result[0]['pts_bbox']['boxes_3d'].bev[result[0]['pts_bbox']['scores_3d'] > show_score_thr]
        bev_box[:, :2] = ((bev_box[:, :2] + 51.2) * big_map_height / 51.2 / 2.0).to(int)
        bev_box[:, 2:4] = (bev_box[:, 2:4] * big_map_height / 51.2 / 2.0).to(int)
        draw_boxes_on_bev(bev_show, boxes=bev_box.cpu().numpy(),thickness=3, attrs=attrs_3d)

        # 转一下 免得右边x只想右侧，而不是上方
        bev_show = np.transpose(bev_show, (1, 0, 2))[:,::-1,:]


        bev_show = bev_show[:,piece_height-int(0.5*piece_width):piece_height+int(0.5*piece_width),:][::-1]

        big_map[:2*piece_height,3 * piece_width:big_map_width] = bev_show

        big_map = cv2.resize(big_map, (video_w, video_h), interpolation=cv2.INTER_AREA)

        # cv2.imshow('ss',big_map)
        # cv2.waitKey(0)

        if show:
            # plt.figure(figsize=(img_front_width, img_front_width))
            plt.imshow(big_map)
            plt.axis('off')  # 关闭坐标轴
            plt.show()

        out.write(big_map)
    out.release()
    print(f'video save in {output_file}')

def main():
    args = parse_args()

    assert args.out or args.eval or args.format_only or args.show \
           or args.show_dir, \
        ('Please specify at least one operation (save/eval/format/show the '
         'results / save the results) with the argument "--out", "--eval"'
         ', "--format-only", "--show" or "--show-dir"')

    if args.eval and args.format_only:
        raise ValueError('--eval and --format_only cannot be both specified')

    if args.out is not None and not args.out.endswith(('.pkl', '.pickle')):
        raise ValueError('The output file must be a pkl file.')

    cfg = Config.fromfile(args.config)
    if args.cfg_options is not None:
        cfg.merge_from_dict(args.cfg_options)

    cfg = compat_cfg(cfg)

    # set multi-process settings
    setup_multi_processes(cfg)

    # set cudnn_benchmark
    if cfg.get('cudnn_benchmark', False):
        torch.backends.cudnn.benchmark = True

    # import modules from plguin/xx, registry will be updated
    if hasattr(cfg, 'plugin'):
        if cfg.plugin:
            import importlib
            if hasattr(cfg, 'plugin_dir'):
                plugin_dir = cfg.plugin_dir
                _module_dir = os.path.dirname(plugin_dir)
                _module_dir = _module_dir.split('/')
                _module_path = _module_dir[0]

                for m in _module_dir[1:]:
                    _module_path = _module_path + '.' + m
                print(_module_path)
                plg_lib = importlib.import_module(_module_path)
            else:
                # import dir is the dirpath for the config file
                _module_dir = os.path.dirname(args.config)
                _module_dir = _module_dir.split('/')
                _module_path = _module_dir[0]
                for m in _module_dir[1:]:
                    _module_path = _module_path + '.' + m
                print(_module_path)
                plg_lib = importlib.import_module(_module_path)

    cfg.model.pretrained = None

    if args.gpu_ids is not None:
        cfg.gpu_ids = args.gpu_ids[0:1]
        warnings.warn('`--gpu-ids` is deprecated, please use `--gpu-id`. '
                      'Because we only support single GPU mode in '
                      'non-distributed testing. Use the first GPU '
                      'in `gpu_ids` now.')
    else:
        cfg.gpu_ids = [args.gpu_id]

    # init distributed env first, since logger depends on the dist info.
    if args.launcher == 'none':
        distributed = False
    else:
        distributed = True
        init_dist(args.launcher, **cfg.dist_params)

    test_dataloader_default_args = dict(
        samples_per_gpu=1, workers_per_gpu=2, dist=distributed, shuffle=False)

    # in case the test dataset is concatenated
    if isinstance(cfg.data.test, dict):
        cfg.data.test.test_mode = True
        if cfg.data.test_dataloader.get('samples_per_gpu', 1) > 1:
            # Replace 'ImageToTensor' to 'DefaultFormatBundle'
            cfg.data.test.pipeline = replace_ImageToTensor(
                cfg.data.test.pipeline)
    elif isinstance(cfg.data.test, list):
        for ds_cfg in cfg.data.test:
            ds_cfg.test_mode = True
        if cfg.data.test_dataloader.get('samples_per_gpu', 1) > 1:
            for ds_cfg in cfg.data.test:
                ds_cfg.pipeline = replace_ImageToTensor(ds_cfg.pipeline)

    test_loader_cfg = {
        **test_dataloader_default_args,
        **cfg.data.get('test_dataloader', {})
    }

    # set random seeds
    if args.seed is not None:
        set_random_seed(args.seed, deterministic=args.deterministic)

    # build the dataloader
    dataset = build_dataset(cfg.data.test)
    data_loader = build_dataloader(dataset, **test_loader_cfg)

    # build the model and load checkpoint
    cfg.model.train_cfg = None
    # cfg._cfg_dict['model']
    # cfg._cfg_dict['model']['test_cfg']['pts']['nms_pre'] = args.nms_pre
    # cfg._cfg_dict['model']['test_cfg']['pts']['nms_thr'] = args.nms_thr
    # cfg['model']['test_cfg']['pts']['score_threshold'] = args.score_thr
    # cfg['model']['pts_bbox_head']['bbox_coder']['score_threshold'] = cfg['model']['test_cfg']['pts']['score_threshold']
    model = build_model(cfg.model, test_cfg=cfg.get('test_cfg'))
    fp16_cfg = cfg.get('fp16', None)
    if fp16_cfg is not None:
        wrap_fp16_model(model)
    checkpoint = load_checkpoint(model, args.checkpoint, map_location='cpu')
    if args.fuse_conv_bn:
        model = fuse_conv_bn(model)
    # old versions did not save class info in checkpoints, this walkaround is
    # for backward compatibility
    if 'CLASSES' in checkpoint.get('meta', {}):
        model.CLASSES = checkpoint['meta']['CLASSES']
    else:
        model.CLASSES = dataset.CLASSES
    # palette for visualization in segmentation tasks
    if 'PALETTE' in checkpoint.get('meta', {}):
        model.PALETTE = checkpoint['meta']['PALETTE']
    elif hasattr(dataset, 'PALETTE'):
        # segmentation dataset has `PALETTE` attribute
        model.PALETTE = dataset.PALETTE

    dataset_type = cfg.dataset_type
    if dataset_type[0:3]=='CYW':
        args.data_type = 'cyw'
    else:
        args.data_type = 'nus'

    model = MMDataParallel(model, device_ids=cfg.gpu_ids)
    outputs = featmap_view(model, data_loader, args.show, args.show_dir, show_score_thr=args.score_thr,
                        # pth_dir=args.checkpoint.replace('.pth','_new.pth'),
                        pth_dir=args.checkpoint,
                           data_type=args.data_type)


if __name__ == '__main__':
    main()
